#include "rosbag2_cpp/writer.hpp"
#include <cstdio>
#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/rclcpp.hpp>

using geometry_msgs::msg::Twist;

class bagWriter : public rclcpp::Node {

  // 记录乌龟轨迹信息
  rclcpp::Subscription<Twist>::SharedPtr sub_;

  std::unique_ptr<rosbag2_cpp::Writer> writer_;

  void do_write_msg(std::shared_ptr<rclcpp::SerializedMessage> msg) {
    // 写入数据
    writer_->write(msg, "/turtle1/cmd_vel", "geometry_msgs/msg/Twist",
                   this->now());
  }

public:
  bagWriter() : rclcpp::Node("bag_writer") {
    writer_ = std::make_unique<rosbag2_cpp::Writer>();

    writer_->open("my_bag"); // 创建bag文件

    sub_ = this->create_subscription<Twist>(
        "/turtle1/cmd_vel", 10,
        std::bind(&bagWriter::do_write_msg, this, std::placeholders::_1));
  }

  ~bagWriter() override { writer_->close(); }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<bagWriter>());
  rclcpp::shutdown();
  return 0;
}
